/**
  ******************************************************************************
  * File Name          : main.c
  * Description        : Main program body
  ******************************************************************************
  *
  * COPYRIGHT(c) 2016 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *   1. Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *   2. Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *   3. Neither the name of STMicroelectronics nor the names of its contributors
  *      may be used to endorse or promote products derived from this software
  *      without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>


#include "spi.h"
#include "i2c.h"
#include "dwOps.h"
#include "cfg.h"
#include "mac.h"

#include "eeprom.h"
#include "led.h"
#include "lcd_oled.h"
#include "serialProtocol.h"

#include "serial.h"
#include "syscalls.h"
#include "rangingProtocol.h"

// global variables
bool usartDataReady  = false;
uint8_t usartReadBuffer[GPS_MSG_LEN*2];
uint8_t usartTransBuffer[100];
gps_info gps_info_buffer;

#define ERRORHANDLE(...)  {}

/* USER CODE BEGIN Includes */
UART_HandleTypeDef huart2;
UART_HandleTypeDef huart1;
/* USER CODE END Includes */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/

static CfgMode gmode = modeAnchor;

const uint8_t *uid = (uint8_t*)0x1FFFF7AC;
//const uint8_t *uid = (uint8_t*)MCU_ID_ADDRESS;

int initDwm1000(void);

#define N_NODES 9

uint8_t address[] = {0,0,0,0,0,0,0xcf,0xbc};
uint8_t base_address[] = {0,0,0,0,0,0,0xcf,0xbc};

// Static system configuration
#define MAX_ANCHORS 6    
// #define MAX_ANCHORS 3        
uint8_t anchors[MAX_ANCHORS];

#define VALID_NODES_LEN 2
uint8_t validNodeList[VALID_NODES_LEN] = {1,2};

// The four packets for ranging
#define POLL 0x01   // Poll is initiated by the tag
#define ANSWER 0x02
#define FINAL 0x03
#define REPORT 0x04 // Report contains all measurement from the anchor
#define ANCHOR_READY 0x05
#define TAG_READY 0x06

//Justin add -- start
//#pragma anon_unions
typedef struct {
	uint8_t tagAddress[8];   //tag�ĵ�ַ
  uint8_t anchorAddress[8];  //��վ��ַ
  uint32_t distance;      //������Ϣ
} __attribute__ ((packed)) reportDistance_t;
//Justin add -- end

typedef struct {
  uint8_t pollRx[5];
  uint8_t answerTx[5];
  uint8_t finalRx[5];

  float pressure;
  float temperature;
  float asl;
  uint8_t pressure_ok;
} __attribute__ ((packed)) reportPayload_t;

//Karl add--start
typedef struct {
	uint16_t airspeed;
  
	float angle1;
	float angle2;
	float angle3;
} Karl_payload;	

	double distance1 = 0;
	double distance2 = 0;
	double distance3 = 0;

//Karl add--end	

#pragma anon_unions   
typedef union timestamp_u {
  uint8_t raw[5];
  uint64_t full;
  struct {
    uint32_t low32;
    uint8_t high8;
  } __attribute__ ((packed));
  struct {
    uint8_t low8;
    uint32_t high32;
  } __attribute__ ((packed));
}__attribute__ ((packed)) timestamp_t;

// Timestamps for ranging    ʱ��� 
dwTime_t poll_tx;
dwTime_t poll_rx;
dwTime_t answer_tx;
dwTime_t answer_rx;
dwTime_t final_tx;
dwTime_t final_rx;

uint32_t rangingTick;

float pressure, temperature, asl;
bool pressure_ok;

const double C = 299792458.0;       // Speed of light
const double tsfreq = 499.2e6 * 128;  // Timestamp counter frequency

#define ANTENNA_OFFSET 154.6   // ����������  In meter
#define ANTENNA_DELAY  (ANTENNA_OFFSET*499.2e6*128)/299792458.0   // In radio tick

packet_t rxPacket;
packet_t txPacket;
static volatile uint8_t curr_seq = 0;

// Sniffer queue
#define QUEUE_LEN 16
packet_t snifferPacketQueue[QUEUE_LEN];
int snifferPacketLength[QUEUE_LEN];
int queue_head = 0;
int queue_tail = 0;
volatile uint32_t dropped = 0;

static void restConfig(void);
static void changeAddress(uint8_t addr);
static void handleInput(char ch);
static void changeMode(CfgMode newMode);
static void printMode(void);
static void help(void);

// #define printf(...)
#define debug(...)
// #define lj_debug(...)  {printf("%d : ",__LINE__); printf(__VA_ARGS__);}
#define lj_debug(...)
#define lj_error(...) 
// #define lj_error(...) {printf("%d : ",__LINE__); printf(__VA_ARGS__);}
//Justin Add
uint32_t timeRangingComplete = 0;
uint32_t getTokentime = 0;
uint8_t rangingComplete = 0;
volatile bool rangingLock = false;//�����ڲ��״̬��ʱ��rangingLock����Ϊtrue
uint32_t rangingLockTick = 0;//
uint8_t rangingTagAddr = 0xFF;//���ڲ���tag��ַ
#define rangingOverTime 50 //over time 10ms,normally measure ranging time about 5ms

#include "RTE_Components.h"             // Component selection
//#define OLEDDISPLAY
#define USE_FTDI_UART

/* String used to display measured distance on LCD screen (16 characters maximum). */
char dist_str[16] = {0};   //Not necessary at all here

//uart packet
uint8_t uartPacket[32] = {0x55,0xAA,};

bool gNextTag = false; // for anchor to be the next tag
bool gNextTagInFinalStep = false; // twice comfirm to the next tag
bool gTransToken = false;//for current tag
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART2_UART_Init(void);

//Karl add-end


#define TYPE 0
#define SEQ 1
#define TOKEN_EXCHANGE 2

#define PAYLOAD_HEADER_LEN 3
#define MAX_WAIT_ANCHOR_READY 30 

void txcallback(dwDevice_t *dev)
{
  dwTime_t departure;
  dwGetTransmitTimestamp(dev, &departure);   // Get time stamp and define 'departure'

  debug("TXCallback: ");

  switch (txPacket.payload[TYPE]) {   //first bit of send packet to determine the type of packet
    case POLL:
      rangingTick = HAL_GetTick();
      lj_debug("POLL\r\n");
      poll_tx = departure;
      break;
    case ANSWER:
      lj_debug("ANSWER to %02x at %04x\r\n", txPacket.destAddress[0], (unsigned int)departure.low32);
      answer_tx = departure;
      break;
    case FINAL:
      lj_debug("FINAL\r\n");
      final_tx = departure;  
      break;
    case REPORT:
      lj_debug("REPORT\r\n");
      break;      //The txpacket mainly transfer the time stamp 
    case ANCHOR_READY:
      lj_debug("anchor ready\n");
      break;
    case TAG_READY:
      lj_debug("tag ready\n");
      break;
    default:
      break;
  }
}

uint64_t distCount = 0;

void rxcallback(dwDevice_t *dev) {
  dwTime_t arival; 
  uint32_t  tempinv;    
  int dataLength = dwGetDataLength(dev);

  if (dataLength == 0) {
    lj_debug("Error \r\n");
    #warning "Not tested on tag mode."
    dwNewReceive(dev);
    dwSetDefaults(dev);
    dwStartReceive(dev);
    return;
  }
	//TODO
	memset(&rxPacket,0,MAC802154_HEADER_LENGTH);

  debug("RXCallback(%d): ", dataLength);

  dwGetData(dev, (uint8_t*)&rxPacket, dataLength);
	
  if (memcmp(rxPacket.destAddress, address, 8)) {
    debug("Not for me! for %02x with %02x\r\n", rxPacket.destAddress[0], rxPacket.payload[0]);
    dwNewReceive(dev);
    dwSetDefaults(dev);
    dwStartReceive(dev);
    return;
  }
	
	if(rangingLock == true && !(rangingTagAddr == rxPacket.sourceAddress[0]))
	{
		debug("ranging with tag %02x,but tag %02x want to rang\r\n", rxPacket.destAddress[0], rangingTagAddr);
    dwNewReceive(dev);
    dwSetDefaults(dev);
    dwStartReceive(dev);
		return;
	}

	debug("ReceiveTimestamp\r\n");
	
  memcpy(txPacket.destAddress, rxPacket.sourceAddress, 8);
  memcpy(txPacket.sourceAddress, rxPacket.destAddress, 8);
	

  switch(rxPacket.payload[TYPE]) {
    case POLL: //anchor
    {
			rangingTagAddr = rxPacket.sourceAddress[0];
			rangingLock = true;
			rangingLockTick = HAL_GetTick();
		
      rangingTick = HAL_GetTick();
      
      //response
      if (rxPacket.payload[TOKEN_EXCHANGE]==1){
         gNextTag= true;
         lj_debug("Poll tells me to be Tag\r\n");
      }

      txPacket.payload[TYPE] = ANSWER;  //Receive POLL and then send ANSWER
      txPacket.payload[SEQ] = rxPacket.payload[SEQ];
      txPacket.payload[TOKEN_EXCHANGE]  = rxPacket.payload[TOKEN_EXCHANGE]; 
		  lj_debug("txPacket.payload[TOKEN_EXCHANGE]: %d \r\n",txPacket.payload[TOKEN_EXCHANGE]);
      dwNewTransmit(dev);
      dwSetDefaults(dev);

      #warning "may have buffer overflow problem"
      dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+PAYLOAD_HEADER_LEN);  //Karl change the size

      dwWaitForResponse(dev, true);
      dwStartTransmit(dev);
		
			dwGetReceiveTimestamp(dev, &arival);
      poll_rx = arival;
      lj_debug("Poll finish\r\n");
			break;
    }
    case ANSWER://tag
    {
      if (rxPacket.payload[SEQ] != curr_seq) {
        lj_error("Wrong sequence number!\r\n");

        return;
      }
			//Karl add--start
			if (rxPacket.sourceAddress[0] == 1){     //Only applied in anchor 3
			//Karl_payload *test = (Karl_payload *)(rxPacket.payload+2);	
      //printf("%x %f %f %f ",test->airspeed,test->angle1,test->angle2,test->angle3);  
				
     // gps_info *gps_report = (gps_info *)(rxPacket.payload+2);	
      // printf("Wrong sequence number!\r\n");
     // printf("%x  %x %x %llx",gps_report->height,gps_report->lat,gps_report->lon,gps_report->timestamp);

				
      }
			//Karl add--end

      txPacket.payload[TYPE] = FINAL;
      txPacket.payload[SEQ] = rxPacket.payload[SEQ];
      
      if( (rxPacket.payload[TOKEN_EXCHANGE]==1 && gTransToken == true) || (rxPacket.payload[TOKEN_EXCHANGE]==0 && gTransToken == false)){
        if(gTransToken==true){
          txPacket.payload[TOKEN_EXCHANGE] = 1;
        }
      }
      else{
        lj_error("errors in token transmit\r\n");
        lj_debug("gTransToken:%d\r\n",gTransToken);
      }   

      dwNewTransmit(dev);
      dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+PAYLOAD_HEADER_LEN);

      dwWaitForResponse(dev, true);
      dwStartTransmit(dev);
			
			dwGetReceiveTimestamp(dev, &arival);
      answer_rx = arival;
      
      lj_debug("ANSWER finish\r\n");
      // lj_debug("change to tag\n");
      break;
    }
    case FINAL://anchor
    {
      reportPayload_t *report = (reportPayload_t *)(txPacket.payload+PAYLOAD_HEADER_LEN);
      
      dwGetReceiveTimestamp(dev, &arival);
      final_rx = arival;

      txPacket.payload[TYPE] = REPORT;
      txPacket.payload[SEQ] = rxPacket.payload[SEQ];
      if((rxPacket.payload[TOKEN_EXCHANGE]==1&&gNextTag == true) || (rxPacket.payload[TOKEN_EXCHANGE]==0&&gNextTag == false)){
        txPacket.payload[TOKEN_EXCHANGE] = rxPacket.payload[TOKEN_EXCHANGE];
        if(gNextTag==false){
          lj_debug("No need to change to tag, finish ranging\r\n");
          rangingTagAddr = 0xFF;
          rangingLock = false;
        }
        else{
          lj_debug("change to tag ready, do not finish\r\n");
          gNextTagInFinalStep = true;
          getTokentime = HAL_GetTick();
        }
      }
      else{
        gNextTag = false;
        gNextTagInFinalStep = false;
        lj_error("some errors in token transmit\r\n");
      }
            
      memcpy(&report->pollRx, &poll_rx, 5);
      memcpy(&report->answerTx, &answer_tx, 5);
      memcpy(&report->finalRx, &final_rx, 5);   //maybe the data cost 5 bits
      report->pressure = pressure;
      report->temperature = temperature;
      report->asl = asl;
      report->pressure_ok = pressure_ok;

      dwNewTransmit(dev);
      dwSetDefaults(dev); // TODO why should set defaults?
      dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+PAYLOAD_HEADER_LEN+sizeof(reportPayload_t));   //header_length = 21

      dwWaitForResponse(dev, true);
      dwStartTransmit(dev);
      lj_debug("txPacket.payload[TOKEN_EXCHANGE]:%d\r\n",txPacket.payload[TOKEN_EXCHANGE]); 
      lj_debug("FINAL finish\r\n"); 
      break;
    }
    case REPORT:
    {
      reportPayload_t *report = (reportPayload_t *)(rxPacket.payload+PAYLOAD_HEADER_LEN);
      double tround1, treply1, treply2, tround2, tprop_ctn, tprop, distance;
			
      debug("REPORT\r\n");

      if (rxPacket.payload[SEQ] != curr_seq) {
        debug("Wrong sequence number!\r\n");
        //__HAL_UART_ENABLE_IT(&huart1,UART_IT_RXNE);
        return;
      }

      memcpy(&poll_rx, &report->pollRx, 5);
      memcpy(&answer_tx, &report->answerTx, 5);
      memcpy(&final_rx, &report->finalRx, 5);

      debug("%02x%08x ", (unsigned int)poll_tx.high8, (unsigned int)poll_tx.low32);
      debug("%02x%08x\r\n", (unsigned int)poll_rx.high8, (unsigned int)poll_rx.low32);
      debug("%02x%08x ", (unsigned int)answer_tx.high8, (unsigned int)answer_tx.low32);
      debug("%02x%08x\r\n", (unsigned int)answer_rx.high8, (unsigned int)answer_rx.low32);
      debug("%02x%08x ", (unsigned int)final_tx.high8, (unsigned int)final_tx.low32);
      debug("%02x%08x\r\n", (unsigned int)final_rx.high8, (unsigned int)final_rx.low32);

      tround1 = answer_rx.low32 - poll_tx.low32;
      treply1 = answer_tx.low32 - poll_rx.low32;
      tround2 = final_rx.low32 - answer_tx.low32;
      treply2 = final_tx.low32 - answer_rx.low32;

      debug("%08x %08x\r\n", (unsigned int)tround1, (unsigned int)treply2);
      debug("\\    /   /     \\\r\n");
      debug("%08x %08x\r\n", (unsigned int)treply1, (unsigned int)tround2);
      tempinv =  (tround1 + tround2 + treply1 + treply2);
      if (tempinv == 0 ){
        tprop_ctn = 0;
      }
      else{
        tprop_ctn = ((tround1*tround2) - (treply1*treply2)) / tempinv;
      }
      

      debug("TProp (ctn): %d\r\n", (unsigned int)tprop_ctn);
      tprop = tprop_ctn/tsfreq;
			debug("tprop : %d\r\n", (unsigned int)tprop);
      distance = C * tprop;
			
			distCount++;

			printf("distance %d: %6dmm, \r\n", rxPacket.sourceAddress[0], (unsigned int)(distance*1000));
			
			switch (rxPacket.sourceAddress[0]) {
				case 1: distance1 = distance*1000; break; 
				case 2: distance2 = distance*1000; break;
				case 3: distance3 = distance*1000; break;
				default:
					break;		
			}
			
			if  (distance1 !=0 && distance2 !=0 && distance3 !=0) {
				//printf("%6d %6d %6d\r\n",(unsigned int)distance1,(unsigned int)distance2,(unsigned int)distance3);
        distance1 = 0;
				distance2 = 0;
				distance3 = 0;  
        //printf("%6d %6d %6d\r\n",(unsigned int)distance1,(unsigned int)distance2,(unsigned int)distance3);				
			}
			#ifdef OLEDDISPLAY
				//OLED_ShowString(0,6,"                 ");//clear the line
				sprintf(dist_str, "DIST: %6dmm", (unsigned int)(distance*1000));

				OLED_ShowString(0,6,(uint8_t*)dist_str);
			#endif

			dwGetReceiveTimestamp(dev, &arival);
      debug("Total in-air time (ctn): 0x%08x\r\n", (unsigned int)(arival.low32-poll_tx.low32));
			//printf("Total in-air time (ctn): %.4f s\r\n", (float)((arival.low32-poll_tx.low32)/tsfreq));  //Karl delete
				
			//Justin add -- start
			rangingComplete = 1;
      //Justin add -- end
      
      if((rxPacket.payload[TOKEN_EXCHANGE] == 1 && gTransToken == true) || (rxPacket.payload[TOKEN_EXCHANGE] == 0 && gTransToken == false) ){
        if(rxPacket.payload[TOKEN_EXCHANGE]==1){
          gmode = modeAnchor;
          gTransToken = false;
          ledOn(ledMode);
          txPacket.payload[TYPE] = ANCHOR_READY;
          txPacket.payload[SEQ] = rxPacket.payload[SEQ];
          txPacket.payload[TOKEN_EXCHANGE] = 1; 
          dwNewTransmit(dev);
          dwSetData(dev, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+PAYLOAD_HEADER_LEN);
    
          dwWaitForResponse(dev, true);
          dwStartTransmit(dev);
    
          lj_debug("REPORT finish\r\n");
          lj_debug("I want to change to anchor\r\n");
        }
      }
      else{
        lj_error("some error in token trans\r\n");
        lj_debug("gTransToken:%d\r\n",gTransToken);
      }

	    lj_debug("REPORT finish\r\n");
      break;
    }
    case ANCHOR_READY:
    {
      if(gNextTag == false || gNextTagInFinalStep == false){
        lj_error("ANCHOR_READY: some error\r\n");
      }
      if(gmode == modeAnchor){
        gNextTag = false;
        gNextTagInFinalStep =false;
        rangingTagAddr = 0xFF;
        rangingLock = false;
        gmode = modeTag;
        ledOff(ledMode);
        lj_debug("I change to Tag\r\n");
      }
      else{
        lj_debug("Error happens\r\n");
      }
      lj_debug("ANCHOR_READY finish\r\n");
      break;
    }
    case TAG_READY:
    {
      
      break;
    }
    default:
    {
      lj_debug("Unknown Mode\n");
      break;
    }
  }
}

bool contains(uint8_t* list, int length, int value)
{
  int i;

  for (i=0; i<length; i++) {
    if (list[i] == value) {
      return true;
    }
  }

  return false;
}


/* USER CODE END 0 */
dwDevice_t dwm_device;
dwDevice_t *dwm = &dwm_device;

int main(void)
{
	int result=0;
  int i=0;
  bool selftestPasses = true;  //��������
	
	uint8_t anchorListSize = 0;

  bool ledState = false;
  uint32_t ledTick = 0;
  
  uint8_t gpsInfoMsgBuffer[GPS_MSG_LEN];
  gps_info  gpsinfo;
  uint8_t tempData;
  uint8_t tempAnchorList[MAX_ANCHORS];
  uint8_t tempAnchorListLen =0;

  uint8_t nextTag= 0;  // next tag's address
 
  /* MCU Configuration----------------------------------------------------------*/
  //HAL_Delay(5000);
  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* Configure the system clock */
  SystemClock_Config();

  /* Initialize all configured peripherals */
  MX_GPIO_Init();       //GPIO����LED
	MX_USART1_UART_Init();   //�������õ�ַ�ͽ�ɫ
	//MX_USART2_UART_Init();
  MX_SPI1_Init();    //����DWM1000
	MX_I2C1_Init();     //����eeprom��IPS25h(ѹ��������,�Ѿ����ã�
	LED_init();
	
  // Light up all LEDs to test
  ledOn(ledRanging);
  ledOn(ledSync);
  ledOn(ledMode);

	
	printf("TEST\t: EEPROM self-test\r\n");
	eepromInit(&hi2c1);
	
  cfgInit();
  if (cfgReadU8(cfgAddress, &address[0])) {
    printf("CONFIG\t: Address is 0x%X\r\n", address[0]);
  } else {
    printf("CONFIG\t: Address not found!\r\n");
  }
  if (cfgReadU8(cfgMode, &gmode)) {
    printf("CONFIG\t: Mode is ");
    switch (gmode) {
      case modeAnchor: printf("Anchor\r\n"); break;
      case modeTag: printf("Tag\r\n"); break;
      case modeSniffer: printf("Sniffer\r\n"); break;
      default: printf("UNKNOWN\r\n"); break;
    }
  } else {
    printf("Device mode: Not found!\r\n");
  }

  if (cfgFieldSize(cfgAnchorlist, &anchorListSize)) {
    if (cfgReadU8list(cfgAnchorlist, (uint8_t*)anchors, anchorListSize)) {
      for(i = 0 ;i<anchorListSize;i++){
         if(anchors[i] != address[0] && contains(validNodeList,VALID_NODES_LEN,anchors[i]) ){
            tempAnchorList[tempAnchorListLen] = anchors[i];
            tempAnchorListLen ++ ;
         }
      }
      anchorListSize = tempAnchorListLen;
      printf("CONFIG\t: Tag mode anchor list (%i): ", anchorListSize);
      for (i = 0; i < anchorListSize; i++) {
        anchors[i] = tempAnchorList[i];
        printf("0x%02X ", anchors[i]);
      } 
      printf("\r\n");
    } else {
      printf("CONFIG\t: Tag mode anchor list: Not found!\r\n");
    }
  }
	
	// printf("TEST\t: Initialize DWM1000 ... \r\n");
	dwInit(dwm, &dwOps);       // Init libdw
  dwOpsInit(dwm);
  result = dwConfigure(dwm); // Configure the dw1000 chip
	//result = dwConfigure(dwm,address); // Configure the dw1000 chip
  if (result == 0) {
    // printf("[OK]\r\n");
    dwEnableAllLeds(dwm);
		LED_on(LEDY_RANGING);
  } else {
    // printf("[ERROR]: %s\r\n", dwStrError(result));
    selftestPasses = false;
		LED_on(LEDG_SYNC);
  }
	
	//HAL_Delay(500);  //500ms��ʱ
	
  ledOff(ledRanging);
  ledOff(ledSync);
  ledOff(ledMode);
	
	//DWM1000���߲�������
	dwTime_t delay = {.full = ANTENNA_DELAY/2};      //delay =16475.679
  dwSetAntenaDelay(dwm, delay);

  dwAttachSentHandler(dwm, txcallback);    //sign in the function 'txcallback'
  dwAttachReceivedHandler(dwm, rxcallback);  //sign in the function 'rxcallback'

  dwNewConfiguration(dwm);
  dwSetDefaults(dwm);
	//dwEnableMode(dwm, MODE_LONGDATA_RANGE_LOWPOWER);	
	//dwEnableMode(dwm, MODE_SHORTDATA_FAST_LOWPOWER);	
	//dwEnableMode(dwm, MODE_LONGDATA_FAST_LOWPOWER);		
	//dwEnableMode(dwm, MODE_SHORTDATA_FAST_ACCURACY);	 
	//dwEnableMode(dwm, MODE_LONGDATA_FAST_ACCURACY);
	//dwEnableMode(dwm, MODE_LONGDATA_RANGE_ACCURACY);
	dwEnableMode(dwm, MODE_Karladd);  //Karl add 850K mode
  dwSetChannel(dwm, CHANNEL_2);
  dwSetPreambleCode(dwm, PREAMBLE_CODE_64MHZ_9);

  dwCommitConfiguration(dwm);

  // printf("SYSTEM\t: Node started ...\r\n");
  // printf("SYSTEM\t: Press 'h' for help.\r\n");
	
	
  // Initialize the packet in the TX buffer
  MAC80215_PACKET_INIT(txPacket, MAC802154_TYPE_DATA);
  txPacket.pan = 0xbccf;

  if (gmode == modeAnchor || gmode == modeSniffer) {
    dwNewReceive(dwm);
    dwSetDefaults(dwm);
    dwStartReceive(dwm);   //directly start receive
  }
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  
  if (HAL_UART_Receive_IT(&huart1,usartReadBuffer,GPS_MSG_LEN*2)!=HAL_OK){
     ERRORHANDLE();
  }

  while (1)
  {
    //serial read
    if( RINGFIFO_EMPTY(gUartFifo) == false)
    {
        while(RINGFIFO_COUNT(gUartFifo)>=GPS_MSG_LEN){
              RINGFIFO_RD(gUartFifo,tempData);
              
              if(tempData == MSG_HEADER)
              {
                  gpsInfoMsgBuffer[0]= tempData;
                  for(i=1;i<GPS_MSG_LEN;i++){
                      RINGFIFO_RD(gUartFifo,tempData);
                      gpsInfoMsgBuffer[i] = tempData;
                  }
                  if(msg_gps_unpack(&gpsinfo,gpsInfoMsgBuffer)==UNPACK_SUCCESS){
                      printf("h");
                      memcpy(&gps_info_buffer,&gpsinfo,sizeof(gps_info));
                  }
                  else{
                      printf("e");
                  }
                  printf("%ld_\n",RINGFIFO_COUNT(gUartFifo));
              }
        }
    }
    if (HAL_UART_Receive_IT(&huart1,usartReadBuffer,GPS_MSG_LEN*2)!=HAL_OK){
        ERRORHANDLE();
    }

// For Tag mode
    if (gmode == modeTag) {
       nextTag = genNextTag(anchors,anchorListSize);

       for (i=0; i<anchorListSize; i++) {
         if (gmode == modeTag){

          base_address[0] = anchors[i];
          dwIdle(dwm);
 
          txPacket.payload[TYPE] = POLL;  //first pachet in a cycle of measurement is 'POLL'
          txPacket.payload[SEQ] = ++curr_seq;
          if(nextTag == anchors[i] ){
            if(i!=anchorListSize-1){
              lj_error("MAIN: error in assign next tag\n");
            }
            lj_debug("Need to transmit Token\r\n");
            txPacket.payload[TOKEN_EXCHANGE] = 1 ;
            gTransToken = true;
          }
          else{
            txPacket.payload[TOKEN_EXCHANGE]=0;
            gTransToken = false;
          }

          memcpy(txPacket.sourceAddress, address, 8);
          memcpy(txPacket.destAddress, base_address, 8);
 
          dwNewTransmit(dwm);
          dwSetDefaults(dwm);
          dwSetData(dwm, (uint8_t*)&txPacket, MAC802154_HEADER_LENGTH+PAYLOAD_HEADER_LEN);
 
          dwWaitForResponse(dwm, true);
          dwStartTransmit(dwm);
 
          //Justin add
          //HAL_Delay(30);
          timeRangingComplete = HAL_GetTick();
          rangingComplete = 0;     //initial the symbol of completation 
          while((rangingComplete ==0)&&(HAL_GetTick()<(timeRangingComplete+rangingOverTime)));

          if(HAL_GetTick()<(timeRangingComplete+rangingOverTime))
          {
           lj_debug("tag and anchor[%d] ranging time: %04d ms\r\n", (i+1),(unsigned int)(HAL_GetTick() - timeRangingComplete));
          }
          else
          {
           lj_debug("anchor[%d] maybe no exist, ranging time over: %04d ms\r\n", (i+1),(unsigned int)(HAL_GetTick() - timeRangingComplete));
          }
          //Justin add end
         }
       }
     }
     else{ 
        if(gmode == modeAnchor && gNextTagInFinalStep == true){
          if(HAL_GetTick()>=  getTokentime + MAX_WAIT_ANCHOR_READY){
            gNextTag = false;
            gNextTagInFinalStep =false;
            rangingTagAddr = 0xFF;
            rangingLock = false;
            gmode = modeTag;
            ledOff(ledMode);
            lj_debug("I change to Tag on timeout\r\n");
          }
        }
     }

		//Here is the end of mode Tag
		
		 if(HAL_GetTick() > rangingLockTick + rangingOverTime && rangingLock ==true)
		 {
        lj_debug("range overTime\r\n");
				rangingLock = false;
				rangingTagAddr = 0xFF;      
		 }

    // Handling of the LEDs
    if (HAL_GetTick() > (ledTick+250)) {
      ledTick = HAL_GetTick();
      ledState = !ledState;   //flash once 250ms
			ledToggle(ledSync);
    }

     if ((HAL_GetTick() < (rangingTick+50)) && ledState) {
       ledOn(ledRanging);
     } else {
       ledOff(ledRanging);
     }

     //ledOff(ledSync);

     switch (gmode) {
       case modeTag:
         ledOff(ledMode);
         break;
       case modeAnchor:
         ledOn(ledMode);
         break;
       case modeSniffer:
         if (ledState) {
           ledOn(ledMode);
         } else {
           ledOff(ledMode);
         }
         break;
       default:
         ledOn(ledMode);
         ledOn(ledSync);
         ledOn(ledRanging);
         break;
     }

  }
  /* USER CODE END 3 */

}

void HAL_UART_RxCpltCallback(UART_HandleTypeDef * uarthandler){
  int i; 
  // printf("f");
  if(uarthandler == & huart1){
    if (HAL_UART_Receive_IT(&huart1,usartReadBuffer,GPS_MSG_LEN*2)!=HAL_OK){
      ERRORHANDLE();
    }
    if ( RINGFIFO_COUNT(gUartFifo) + GPS_MSG_LEN*2 < RINGFIFO_SIZE){
        for(i =0; i< GPS_MSG_LEN*2; i++ ){
            RINGFIFO_WR(gUartFifo,usartReadBuffer[i]);
        }
    }
    else{
      // printf("f");
      ERRORHANDLE();
    }
  }
}

#ifndef min
#define min(a,b) (((a) < (b)) ? (a) : (b))
#endif
void HAL_SYSTICK_Callback(void){
  // every 10 ms transmit transbuffer
  static uint32_t count8ms=0;
  int i ; 
  int len; 
  if(HAL_GetTick()>>3 > count8ms){
    count8ms = HAL_GetTick()>>3; 
    if( RINGFIFO_COUNT(gUartTransmitFifo)!=0){
      len = min(RINGFIFO_COUNT(gUartTransmitFifo),100); 
      
      for(i=0;i<len;i++){
         RINGFIFO_RD(gUartTransmitFifo,usartTransBuffer[i]);
      }
      if (HAL_UART_Transmit_IT(&huart1,usartTransBuffer,len)!=HAL_OK){
          ERRORHANDLE();
      } 
    }
  }
}

// void HardFault_Handler(){
//    while(1){};
//  }
/** System Clock Configuration
*/
void SystemClock_Config(void)
{

  RCC_OscInitTypeDef RCC_OscInitStruct;
  RCC_ClkInitTypeDef RCC_ClkInitStruct;

  __PWR_CLK_ENABLE();

  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = 16;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 8;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  HAL_RCC_OscConfig(&RCC_OscInitStruct);

  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
  HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);

  HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);

  HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

  /* SysTick_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(SysTick_IRQn, 15, 0);
}

/** Configure pins as 
        * Analog 
        * Input 
        * Output
        * EVENT_OUT
        * EXTI
*/
void MX_GPIO_Init(void)
{

  GPIO_InitTypeDef GPIO_InitStruct;

  /* GPIO Ports Clock Enable */
  __GPIOH_CLK_ENABLE();
  __GPIOA_CLK_ENABLE();
  __GPIOB_CLK_ENABLE();
  __GPIOC_CLK_ENABLE();

  /*Configure GPIO pin : PB0 */
  GPIO_InitStruct.Pin = GPIO_PIN_0;
  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

  /*Configure GPIO pins : PC10 PC11 PC12 */
  GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_LOW;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

  /*Configure GPIO pin : PA4 */
  GPIO_InitStruct.Pin = GPIO_PIN_4;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);

}


/* USART1 init function */
void MX_USART1_UART_Init(void)
{
	
  huart1.Instance = USART1;
  huart1.Init.BaudRate = 230400;     //karl change from 115200 to 230400
  huart1.Init.WordLength = UART_WORDLENGTH_8B;  
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
  huart1.Init.Mode = UART_MODE_TX_RX;
  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
  HAL_UART_Init(&huart1);
	

}

/* USART2 init function */
void MX_USART2_UART_Init(void)
{

  huart2.Instance = USART2;
  huart2.Init.BaudRate = 115200;                  //serial data rate
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  HAL_UART_Init(&huart2);

}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

#ifdef USE_FULL_ASSERT

/**
   * @brief Reports the name of the source file and the source line number
   * where the assert_param error has occurred.
   * @param file: pointer to the source file name
   * @param line: assert_param error line source number
   * @retval None
   */
void assert_failed(uint8_t* file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
    ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */

}


#endif

static void handleInput(char ch) {
  bool configChanged = true;

  switch (ch) {
    case '0':
    case '1':
    case '2':
    case '3':
    case '4':
    case '5':
    case '6':
    case '7':
    case '8':
    case '9':
      changeAddress(ch - '0');
      break;
    case 'a': changeMode(modeAnchor); break;
    case 't': changeMode(modeTag); break;
    case 's': changeMode(modeSniffer); break;
    case 'd': restConfig(); break;
    case 'h':
      help();
      configChanged = false;
      break;
    case '#':
      //productionTestsRun();
      // printf("System halted, reset to continue\r\n");
      //while(true){}  //Karl delete
      break;
    default:
      configChanged = false;
      break;
  }

  if (configChanged) {
    // printf("EEPROM configuration changed, restart for it to take effect!\r\n");
  }
}

static void restConfig(void) {
  // printf("Resetting EEPROM configuration...");
  if (cfgReset()) {
    // printf("OK\r\n");
  } else {
    // printf("ERROR\r\n");
  }
}

static void changeAddress(uint8_t addr) {
  // printf("Updating address from 0x%02X to 0x%02X\r\n", address[0], addr);
  cfgWriteU8(cfgAddress, addr);
  if (cfgReadU8(cfgAddress, &address[0])) {
    // printf("Device address: 0x%X\r\n", address[0]);
  } else {
    // printf("Device address: Not found!\r\n");
  }
}

static void changeMode(CfgMode newMode) {
    // printf("Previous device mode: ");
    printMode();

    cfgWriteU8(cfgMode, newMode);

    // printf("New device mode: ");
    printMode();
}

static void printMode(void) {
  CfgMode mode;

  if (cfgReadU8(cfgMode, &mode)) {
    // switch (mode) {
    //   case modeAnchor: printf("Anchor"); break;
    //   case modeTag: printf("Tag"); break;
    //   case modeSniffer: printf("Sniffer"); break;
    //   default: printf("UNKNOWN"); break;
    // }
  } else {
    // printf("Not found!");
  }

  // printf("\r\n");
}

static void help(void) {
  // printf("Help\r\n");
  // printf("-------------------\r\n");
  // printf("0-9 - set address\r\n");
  // printf("a   - anchor mode\r\n");
  // printf("t   - tag mode\r\n");
  // printf("s   - sniffer mode\r\n");
  // printf("d   - reset configuration\r\n");
  // printf("h   - This help\r\n");
}



/**
  * @}
  */ 

/**
  * @}
*/ 

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
